#ifndef JACOBIAN_VERIFIER_H
#define JACOBIAN_VERIFIER_H

#include <Eigen/Dense>
#include <iostream>
#include <cmath>

class JacobianVerifier {
public:
    static void VerifyReprojectionJacobian(const Eigen::Vector3d& landmark,
                                          const Eigen::Vector2d& bearing,
                                          const Eigen::Vector3d& estimated_translation,
                                          const Eigen::Quaterniond& estimated_rotation,
                                          double fx, double fy, double cx, double cy) {
        std::cout << "\n=== JACOBIAN NUMERICAL VERIFICATION ===" << std::endl;
        
        // 测试参数
        // double translation[3] = {0.1, -0.2, 0.3};
        // double quaternion[4] = {0.2, 0.1, 0.3, 0.923}; // 注意：Eigen顺序是[x,y,z,w]
        double translation[3] = {estimated_translation[0], estimated_translation[1], estimated_translation[2]};
        double quaternion[4] = {estimated_rotation.x(), estimated_rotation.y(), estimated_rotation.z(), estimated_rotation.w()}; // 注意：Eigen顺序是[x,y,z,w]
        
        // 归一化四元数
        Eigen::Map<Eigen::Quaterniond> q(quaternion);
        q.normalize();
        
        std::cout << "Test point: " << landmark.transpose() << std::endl;
        std::cout << "Test observation: " << bearing.transpose() << std::endl;
        std::cout << "Test translation: " << translation[0] << ", " << translation[1] << ", " << translation[2] << std::endl;
        std::cout << "Test quaternion: " << quaternion[0] << ", " << quaternion[1] << ", " << quaternion[2] << ", " << quaternion[3] << std::endl;
        
        // 1. 计算解析雅可比
        double analytic_residuals[2];
        double analytic_jacobians[2][8]; // [trans(6) + quat(8)]
        double* analytic_params[2] = {translation, quaternion};
        double* analytic_jacobians_ptr[2] = {analytic_jacobians[0], analytic_jacobians[1]};
        
        AnalyticReprojectionError analytic_cost(landmark, bearing, fx, fy, cx, cy);
        analytic_cost.Evaluate(analytic_params, analytic_residuals, analytic_jacobians_ptr);
        
        // 2. 计算数值雅可比
        double numerical_jac_trans[6]; // 2x3
        double numerical_jac_quat[8];  // 2x4
        
        ComputeNumericalJacobian(landmark, bearing, fx, fy, cx, cy, 
                                translation, quaternion,
                                numerical_jac_trans, numerical_jac_quat);
        
        // 3. 比较结果
        std::cout << "\n--- TRANSLATION JACOBIAN COMPARISON ---" << std::endl;
        CompareJacobians("Translation", analytic_jacobians[0], numerical_jac_trans, 2, 3);
        
        std::cout << "\n--- QUATERNION JACOBIAN COMPARISON ---" << std::endl;
        CompareJacobians("Quaternion", analytic_jacobians[1], numerical_jac_quat, 2, 4);
        
        // 4. 残差验证
        std::cout << "\n--- RESIDUAL VERIFICATION ---" << std::endl;
        std::cout << "Residuals: [" << analytic_residuals[0] << ", " << analytic_residuals[1] << "]" << std::endl;
    }

private:
    static void ComputeNumericalJacobian(const Eigen::Vector3d& landmark,
                                        const Eigen::Vector2d& bearing,
                                        double fx, double fy, double cx, double cy,
                                        const double* translation,
                                        const double* quaternion,
                                        double* jac_trans, double* jac_quat) {
        const double kEpsilon = 1e-7;
        
        // 基准残差
        double base_residuals[2];
        ComputeResiduals(landmark, bearing, fx, fy, cx, cy, translation, quaternion, base_residuals);
        
        // 平移的数值雅可比
        double trans_perturbed[3];
        std::copy(translation, translation + 3, trans_perturbed);
        
        for (int i = 0; i < 3; ++i) {
            trans_perturbed[i] += kEpsilon;
            double perturbed_residuals[2];
            ComputeResiduals(landmark, bearing, fx, fy, cx, cy, trans_perturbed, quaternion, perturbed_residuals);
            
            for (int j = 0; j < 2; ++j) {
                jac_trans[j * 3 + i] = (perturbed_residuals[j] - base_residuals[j]) / kEpsilon;
            }
            trans_perturbed[i] = translation[i]; // 恢复
        }
        
        // 四元数的数值雅可比
        double quat_perturbed[4];
        std::copy(quaternion, quaternion + 4, quat_perturbed);
        
        for (int i = 0; i < 4; ++i) {
            quat_perturbed[i] += kEpsilon;
            
            // 归一化扰动后的四元数
            Eigen::Map<Eigen::Quaterniond> q_perturbed(quat_perturbed);
            q_perturbed.normalize();
            
            double perturbed_residuals[2];
            ComputeResiduals(landmark, bearing, fx, fy, cx, cy, translation, quat_perturbed, perturbed_residuals);
            
            for (int j = 0; j < 2; ++j) {
                jac_quat[j * 4 + i] = (perturbed_residuals[j] - base_residuals[j]) / kEpsilon;
            }
            quat_perturbed[i] = quaternion[i]; // 恢复
        }
    }
    
    static void ComputeResiduals(const Eigen::Vector3d& landmark,
                                const Eigen::Vector2d& bearing,
                                double fx, double fy, double cx, double cy,
                                const double* translation,
                                const double* quaternion,
                                double* residuals) {
        Eigen::Map<const Eigen::Quaterniond> q(quaternion);
        Eigen::Matrix3d R = q.toRotationMatrix();
        
        Eigen::Vector3d p_c = R * landmark + Eigen::Map<const Eigen::Vector3d>(translation);
        
        double x_c = p_c[0];
        double y_c = p_c[1];
        double z_c = std::max(p_c[2], 1e-8); // 避免除零
        
        double x_n = x_c / z_c;
        double y_n = y_c / z_c;
        
        double u_pred = fx * x_n + cx;
        double v_pred = fy * y_n + cy;
        
        residuals[0] = bearing[0] - u_pred;
        residuals[1] = bearing[1] - v_pred;
    }
    
    static void CompareJacobians(const std::string& name,
                                const double* analytic_jac,
                                const double* numerical_jac,
                                int rows, int cols) {
        std::cout << "Row/Col ";
        for (int c = 0; c < cols; ++c) {
            std::cout << "  Col" << c << "    ";
        }
        std::cout << std::endl;
        
        double max_relative_error = 0.0;
        int max_error_row = -1, max_error_col = -1;
        
        for (int r = 0; r < rows; ++r) {
            std::cout << "Row" << r << "  ";
            for (int c = 0; c < cols; ++c) {
                int idx = r * cols + c;
                double analytic = analytic_jac[idx];
                double numerical = numerical_jac[idx];
                double abs_error = std::abs(analytic - numerical);
                double rel_error = (std::abs(numerical) > 1e-10) ? abs_error / std::abs(numerical) : abs_error;
                
                // 颜色编码：绿色=好，黄色=警告，红色=错误
                const char* color = "\033[32m"; // 绿色
                if (rel_error > 0.1) color = "\033[31m"; // 红色
                else if (rel_error > 0.01) color = "\033[33m"; // 黄色
                
                printf("%s%8.3f\033[0m ", color, rel_error);
                
                if (rel_error > max_relative_error) {
                    max_relative_error = rel_error;
                    max_error_row = r;
                    max_error_col = c;
                }
            }
            std::cout << std::endl;
            
            // 显示具体数值（可选）
            std::cout << "  Analytic ";
            for (int c = 0; c < cols; ++c) {
                int idx = r * cols + c;
                printf("%8.3f ", analytic_jac[idx]);
            }
            std::cout << std::endl;
            
            std::cout << "  Numeric  ";
            for (int c = 0; c < cols; ++c) {
                int idx = r * cols + c;
                printf("%8.3f ", numerical_jac[idx]);
            }
            std::cout << std::endl;
        }
        
        std::cout << "\nMax relative error: " << max_relative_error;
        std::cout << " at (" << max_error_row << "," << max_error_col << ")" << std::endl;
        
        if (max_relative_error < 0.01) {
            std::cout << "✓ Jacobian validation PASSED - good agreement" << std::endl;
        } else if (max_relative_error < 0.1) {
            std::cout << "⚠ Jacobian validation WARNING - moderate errors" << std::endl;
        } else {
            std::cout << "✗ Jacobian validation FAILED - large errors" << std::endl;
        }
    }
};

#endif // JACOBIAN_VERIFIER_H